/*
 * MotorController.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 */

/*Belongs to MotionPlanner*/
#include "MotorController.h"
#include <string>
#include <sstream>
#include <stdlib.h>
#include <vector>
#include <iomanip>
#include <cmath>
#include <iostream>
#include <fstream>

//#include "Dyna.h"

#include <dynamixel.h>


#define NUM_MOTORS	12
#define MOTIONS_TOTAL 9
#define SPEED_CONSTANT	0.1285
#define INITIAL_SPEED	15

#define CHECK_POSITIONS		6
#define POSITION_TOLERANCE	25
#define P_GAIN		28	// This is the address of the P value, DO NOT CHANGE

#define DEGREES_PER_POSITION	0.08789
#define RADIANS_PER_DEGREE		0.01745
#define P_FOR_PID	32	// Change this value to change the P value
#define NEW_RETURN_DELAY_TIME	10
#define STATUS_RETURN			1	// 0 returns nothing, 1 returns only read, 2 returns everything


clock_t lastClock, batteryClock;
std::vector<Motion> motions;
int zeroPose[NUM_MOTORS];
int currentPose[NUM_MOTORS];
int currentMotion;

// TODO add these motor status/battery checks to the final framework
double batteryLevel= 0.00;
int temperature= 0;
int checkBattery;
int result;

int motors[NUM_MOTORS];
std::vector<int> data;

//Dyna dynamixel;

//TODO adding another file for creating a new motion. Will be called ANIMATION_FILE
const char* motion_files[] = {STAND_FILE, WALK_FILE, TURN_LEFT_FILE, TURN_RIGHT_FILE,
		SIDE_STEP_LEFT_FILE, SIDE_STEP_RIGHT_FILE, KICK_FILE, ANIMATION_FILE, TEMP_ANIMATION_FILE};

// Default Constructor
MotorController::MotorController() {
	// Initialize USB2Dynamixel
	//	motor_dynamixel.dxl_initialize(0,1);
	dxl_initialize(0,1);
	//Initialize the motors
	initialize();
	// Set the current motion to standing
	currentMotion = STAND;

	for (int i = 0; i < NUM_MOTORS; i++) {
		motors[i] = i + 1;
	}

	data.resize(NUM_MOTORS);
}

// Default Destructor
MotorController::~MotorController() {
	//	motor_dynamixel.dxl_terminate();
	dxl_terminate();
}
void MotorController::initialize() {
	lastClock = clock();
	batteryClock = clock();

	changePID();
	setStatusReturnLevel(STATUS_RETURN);
	setReturnDelayTime();
	initializeMotion();

	// We have to initialize the currentMotion, so we'll let it be STAND
	currentMotion = STAND;
}
//Initialize the Motions. Called from initialize()
void MotorController::initializeMotion() {

	// Define an input file stream, and open the number of motions file
	std::ifstream motionFile;

	// String variable to hold one line at a time from the text files
	std::string aString;


	// The number of steps in a particular motion
	int numMotions;

	//Open a file that specifies the number of motions we need to initialize
	motionFile.open(MOTION_FILE);

	//Let's get the total number of motion files
	//Currently this is not used, but should be later on for knowing how many motions to add to motions vector
	//TODO test this code
	getline(motionFile, aString);
	std::stringstream(aString)>>numMotions;

	//TODO added another motion file for experimental animation_file
	numMotions = MOTIONS_TOTAL;

	std::cout << "Number of motions: " << numMotions <<"\n"<< std::endl;	//TODO Do we need this print statement?

	// Resize the motions vector
	motions.resize(numMotions);

	// Initialize each motion
	for (int i = 0; i < numMotions; i++)
	{
		motions[i] = getInitializedMotion((const char* )motion_files[i]);
	}
	getZeroPose();
}

/* reads from the zero pose of the robot for reading positions as degrees/radians as well as mirroring. */
void MotorController::getZeroPose(){

	std::ifstream file;
	file.open(ZERO_POSE_FILE);
	std::string aString;

	for(int i=0; i<LEG_CUTOFF; i++){
		// each line holds the zero position of a motor. Read each line to the index of the zeroPose array
		getline(file, aString);
		std::stringstream(aString)>>zeroPose[i];

	}
}
/*
 * Initializes the motion associated with the file path passed in.
 * @param motion_file The file path for the motion file
 * @return tempMotion The initialized motion struct
 * Called from initializeMotion().
 */
Motion MotorController::getInitializedMotion(const char* motion_file) {

	Motion tempMotion;
	std::ifstream file;
	// Example: Open the WALK_FILE from which to read the WALK motion

	file.open(motion_file);
	int numSteps = 0;
	std::string aString;
	double tempTime;
	std::string spaceChar 	(" ");
	std::string tabChar	("\t");
	std::string enterChar	("\n");

	// Code for this motion
	getline(file, aString);				// The first number in the file is the total number of steps
	std::stringstream(aString)>>numSteps;

	std::cout << "Motion Contains "<< numSteps << " steps" <<std::endl;

	// Set the length, and resize the time and first position and velocity vectors
	tempMotion.length = numSteps;
	tempMotion.currentIndex = 0;
	tempMotion.motorPositions.resize(numSteps);
	tempMotion.motorVelocities.resize(numSteps);
	tempMotion.time.resize(numSteps);

	// Loop through the second position and velocity vectors, setting their size equal to the number of motors
	for (int i = 0; i < numSteps; i++)
	{
		tempMotion.motorPositions[i].resize(NUM_MOTORS);
		tempMotion.motorVelocities[i].resize(NUM_MOTORS);
	}

	// Initialize the time, position, and velocity data fields
	for (int i = 0; i < numSteps; i++)
	{
		// Read and store the time
		getline(file, aString);
		if (aString.find(tabChar) == 0 || aString.find(enterChar) == 0)
		{
			getline(file, aString);
		}

		std::stringstream(aString) >> tempTime;
		tempMotion.time[i] = tempTime;

		for (int j = 0; j < LEG_CUTOFF; j++)
		{
			// Initialize the motor positions and velocities from the text file
			getline(file, aString);
			if (aString.find(tabChar) == 0 || aString.find(enterChar) == 0)
			{
				getline(file, aString);
			}	

			// Find the location of the space character
			unsigned space = aString.find(tabChar);

			// Push the first number into the motor position. In [i=rows][j=columns], i is the step number of the motion we're in and j is the motor number
			std::stringstream(aString.substr(0, space))>> tempMotion.motorPositions[i][j];

			// Push the second number into the velocity position
			std::stringstream(aString.substr(space+1)) >> tempMotion.motorVelocities[i][j];
		}
		std::cout << "step "<< i << " built" <<std::endl;
	}
	std::cout << "Motion Built ^\n "<<std::endl;
	// Close the text file
	file.close();

	// Return the newly initialized motion
	return tempMotion;

}

// Returns the time in seconds since the lastClock parameter
float MotorController::timeSince(float lastClock) {
	// Return the number of seconds since the last clock was taken
	return (((float)(clock() - lastClock)/CLOCKS_PER_SEC));
}

/**
 *  Code to actually execute the next step
 *	called from MotorController.step(false)
 */
void MotorController::executeNext(Motion motion) {

	// Let the user know that the next step is being executed
	// TODO Delete this functionality for final robot

	if (motion.currentIndex == 0)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = LOW_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, BYTE, data);
	}
	else if (motion.currentIndex == 1)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = DEFAULT_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, WORD, data);
	}

	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorVelocities[motion.currentIndex][i];
	}
	sendSyncWrite(motors, NUM_MOTORS, MOVING_SPEED, WORD, data);

	// Write the goal positions to each of the motors
	// TODO Change this so it includes the whole body

	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorPositions[motion.currentIndex][i];
	}
	sendSyncWrite(motors, NUM_MOTORS, GOAL_POSITION, WORD, data);
	if(motion.currentIndex!=0){
		std::cout << "Step " << motion.currentIndex-1<< " took " << (float) timeSince((float) lastClock) << " seconds." << std::endl;
	}
	std::cout <<"\nStep "<< motion.currentIndex<< " of "<< motion.length-1<< " should take " << motion.time[motion.currentIndex] << " seconds: " <<std::endl;

	/*show results of the step. Did the motors go where they were supposed to? also show the speeds. formatted for easy reading*/
	for (int i = 0; i < NUM_MOTORS; i++) {
		if(i<9){
			std::cout <<"\tMotor  "<< i+1<< "  Goal Position:\t" <<data[i]<<"\tActual Position:\t"<< getMotorPositionReadWord(i+1)<<"\tSpeed:\t"<<motion.motorVelocities[motion.currentIndex][i]<< std::endl;
		}
		else{
			std::cout <<"\tMotor "<< i+1<< "  Goal Position:\t" <<data[i]<<"\tActual Position:\t"<< getMotorPositionReadWord(i+1)<<"\tSpeed:\t"<<motion.motorVelocities[motion.currentIndex][i]<< std::endl;

		}
	}
	// Reset the clock
	lastClock = clock();
}



/*
 * Executes one non-blocking check to see if next step should be executed.
 * @param hasFallen A boolean value to check whether the robot has fallen,
 * and if so, if the proper procedure has been followed.
 * @return Returns true if motion has finished, otherwise returns false.
 */
bool MotorController::step(bool isFalling) {
	// TODO Add error checking to see if robot has fallen
	// If it hasint  fallen, make sure that it's currently standing steady

	// What we are going to return
	bool returnVar = false;

	if (isFalling)
	{
		// Relax all motors
		for (int i = 0; i < NUM_MOTORS; i++)
		{
			//			motor_dynamixel.dxl_write_byte(i+1, TORQUE_ENABLE, 0);
			dxl_write_byte(i+1, TORQUE_ENABLE, 0);
		}

		// Wait for fall to finish
		sleep(5000);

		// TODO add code for getting back up

		return false;

	}

	else //do this is if robot is not falling
	{

		// If it's been longer than the time we have to wait, let's do the next movement or step

		if (timeSince((float)lastClock) >= motions[currentMotion].time[motions[currentMotion].currentIndex-1])
		{
			//If motion is over
			if (motions[currentMotion].currentIndex == motions[currentMotion].length)
			{


				//TODO check the temp of each motor and the battery level. Battery level is not very accurate

				if(checkBattery==1){
					batteryLevel=0; //start fresh
					for(int i=0; i<NUM_MOTORS; i++){
						batteryLevel+= (double) (dxl_read_byte(i, 42)*.1);
					}

					std::cout <<std::endl<< "Battery Voltage ~ ";
					std::cout.setf( std::ios::fixed, std:: ios::floatfield ); // floatfield set to fixed
					std::cout << batteryLevel/NUM_MOTORS << "\n\n";

					for (int i = 0; i < NUM_MOTORS; i++)
					{
						//			motor_dynamixel.dxl_write_byte(i+1, TORQUE_ENABLE, 0);

						temperature= (int) dxl_read_byte(i+1, 43);
						if(i<9){
							std::cout <<"Motor  "<<i+1<<": "<< temperature <<"C"<<std::endl;
						}
						else{
							std::cout <<"Motor "<<i+1<<": "<< temperature <<"C"<<std::endl;

						}
					}
					std::cout <<std::endl;

					checkBattery=0;	//don't allow the battery to be checked until it is set to 1 again(when another motion executes)
					//return voltage > VOLTAGE_THRESHHOLD;
				}
				// We have finished moving, so we'll let the caller know
				returnVar =  true;
			}
			//Motion is not over
			else
			{
				checkBattery= 1;	//lets allow the battery to be checked
				// Execute the next step
				executeNext(motions[currentMotion]);
				// Increment the motion counter
				motions[currentMotion].currentIndex++;
			}

			// Return the correct value;
			return returnVar;
		}

		return returnVar;

	}
}
/**
 * Motion is set here from Main
 */

void MotorController::setMotion(int newMotion) {
	currentMotion = newMotion;
	motions[currentMotion].currentIndex = 0;
}

// Get the currently executing motion
int MotorController::getMotion() {
	return currentMotion;
}

void MotorController::sendSyncWrite(int ids[], int idsLength, int address, int instruction_length, std::vector<int> data) {
	// Make syncwrite packet


	dxl_set_txpacket_id(BROADCAST_ID);//ID of destination
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);// packet type
	dxl_set_txpacket_parameter(0, address);//which data to manipulate. speed, goal pos, ect
	dxl_set_txpacket_parameter(1, instruction_length);//how long the instruction will be. 2 for word, 1 for byte,
	for(int i=0; i<idsLength; i++ )
	{
		// Set the ID number
		dxl_set_txpacket_parameter(2+(instruction_length+1)*i, ids[i]);
		// Set the data values
		if (instruction_length == 1) {
			dxl_set_txpacket_parameter(2+2*i+1, data[i]);
		}

		else if (instruction_length == 2) {
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(data[i]));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(data[i]));
		}
	}
	dxl_set_txpacket_length((instruction_length+1)*idsLength+4);//(2+1) for writing a word, (1+1) for writing a byte, 4 is a constant

	dxl_txrx_packet();//sends the packet
	result = dxl_get_result( );

	if( result == COMM_TXSUCCESS )

	{
		std::cout << "COMM_TXSUCCESS "<<std::endl;
	}

	else if( result == COMM_RXSUCCESS )

	{
		std::cout << " "<<std::endl;
	}

	else if( result == COMM_TXFAIL )

	{
		std::cout << "COMM_TXFAIL "<<std::endl;
	}

	else if( result == COMM_RXFAIL)

	{
		std::cout << "COMM_RXFAIL "<<std::endl;
	}

	else if( result == COMM_TXERROR )

	{
		std::cout << "COMM_TXERROR "<<std::endl;
	}

	else if( result == COMM_RXWAITING )

	{
		std::cout << "COMM_RXWAITING "<<std::endl;
	}
}

Motion MotorController::getMotionFile() {
	return motions[getMotion()];
}

void MotorController::lockHead(){
	dxl_write_byte(24, TORQUE_ENABLE, 1);
}

bool MotorController::incrementStep() {
	// TODO Add error checking to see if robot has fallen
	// If it hasint  fallen, make sure that it's currently standing steady

	// What we are going to return
	bool returnVar1 = false;

	//If motion is over
	if (motions[currentMotion].currentIndex == motions[currentMotion].length)
	{
		returnVar1 =  true;
	}

	//Motion is not over
	else
	{

		// Execute the next step
		executeNext(motions[currentMotion]);
		// Increment the motion counter
		motions[currentMotion].currentIndex++;
	}

	// Return the correct value;
	return returnVar1;

}



void MotorController::incrementIndex() {
	motions[currentMotion].currentIndex++;
}



int MotorController::getMotorPositionReadWord(int id) {
	return dxl_read_word(id, PRESENT_POSITION);
}

std::vector<int> MotorController::getMotorPositionSyncRead(int numMotors) {
	std::vector<int> positions;
	positions.resize(numMotors);
	int ids[numMotors];

	for (int i = 0; i < numMotors; i++) {
		ids[i] = i+1;
		std::cout << ids[i] << " ";
	}

	std::cout << std::endl;


	dxl_set_txpacket_id(0xFD);//ID of destination
	dxl_set_txpacket_instruction(INST_SYNC_READ);// packet type
	dxl_set_txpacket_parameter(0, PRESENT_POSITION);//which data to manipulate. speed, goal pos, ect
	dxl_set_txpacket_parameter(1, BYTE);//how long the instruction will be.

	for(int i=0; i<numMotors; i++ )
	{
		// Set the ID number
		dxl_set_txpacket_parameter(2+i, ids[i]);

	}

	dxl_set_txpacket_length(numMotors+4);

	dxl_txrx_packet();//sends the packet

	int high;
	int low;
	std::cout << "Total packets: " << dxl_get_rxpacket_length() << std::endl;

	for (int i = 0; i < numMotors; i++) {
		low = dxl_get_rxpacket_parameter(2*i);
		std::cout << "Low: " << low << "\t\t";
		high = dxl_get_rxpacket_parameter(2*i+1);
		std::cout << "High: " << high << "\t\t" << i << std::endl;
		positions[i] = dxl_makeword(low, high);
	}



	return positions;
}



void MotorController::executePrevious(Motion motion) {
	// Let the user know that the next step is being executed
	// TODO Delete this functionality for final robot
	if (motion.currentIndex <= 1) { return; }
	motion.currentIndex -= 2; //This is because the currentIndex is already set to the next motion, so we need to set it to the previous motion
	std::cout << "Executing: " << motion.currentIndex <<  " Time since last execution = " << (float) timeSince((float) lastClock) << std::endl;
	std::cout << "Motion should take " << motion.time[motion.currentIndex] << " seconds." << std::endl;

	if (motion.currentIndex == 0)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = LOW_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, BYTE, data);
	}
	else if (motion.currentIndex == 1)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = DEFAULT_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, WORD, data);
	}
	// Write the new velocities to each of the motors
	// TODO Change this so it includes the whole body
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorVelocities[motion.currentIndex][i];
	}
	sendSyncWrite(motors, NUM_MOTORS, MOVING_SPEED, WORD, data);

	// Write the goal positions to each of the motors
	// TODO Change this so it includes the whole body
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorPositions[motion.currentIndex][i];
		std::cout << motion.motorPositions[motion.currentIndex][i] << "\tPresent Position: " << std::endl;
	}
	sendSyncWrite(motors, NUM_MOTORS, GOAL_POSITION, WORD, data);

	// Reset the clock
	lastClock = clock();

	motion.currentIndex++;
}

void MotorController::disableAllMotors() {
	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = 0;
	}

	sendSyncWrite(motors, NUM_MOTORS, TORQUE_ENABLE, BYTE, datas);
}

//TODO for editing new steps, the current positions are save to the struct if user wants to save step
void MotorController::enableAllMotors() {

	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = 1;
	}
	sendSyncWrite(motors, NUM_MOTORS, TORQUE_ENABLE, BYTE, datas);
	std::cout << "\nStep\n" << motions[currentMotion].currentIndex << "\n\n";
	//sendSyncWrite(motors, NUM_MOTORS, TORQUE_ENABLE, BYTE, datas); //TODO double tapping this write method to see if motor positions are more accurate
	getCurrentPose();

}

/*Will run a error checking algorithm to make sure that the motors aren't reading back unrealistic values */
void MotorController::getCurrentPose(){
	int tempPositions[CHECK_POSITIONS];

	std::vector<int> badMotorList;//hold a list of the motors that gave bad readings on the forward read
	int badMotorSize=1; //to resize the list
	bool reverseCheck;
	bool badPose;
	int tempRevPositions[NUM_MOTORS];


	for (int i = 0; i < NUM_MOTORS; i++) {
		/*Going to take a certain number of samples from this motor*/
		for(int j= 0; j<CHECK_POSITIONS; j++){
			//compare indexes 0 and 1. Are they within 11 (1 degree) of eachother? good, lets compare 2 to be sure
				tempPositions[j]= dxl_read_word(i+1, PRESENT_POSITION);
				std::cout<<tempPositions[j]<<"  ";
		}
		currentPose[i] = errorCheck(tempPositions, CHECK_POSITIONS);//holds value of the error checking results


		if(currentPose[i]<0){
			reverseCheck= true;
			//lets signal that we're going to read the motors in the reverse order to get better values
			badMotorList.resize(badMotorSize++);
			badMotorList[badMotorSize-2]= i;
		}
		/*Check to see that motor position is within the CW CCW limits*/
		if(getBoundedPosition(currentPose[i], i+1)<0){
						std::cout<<"Motor "<<i<<" "<<"is outside of its range of motion!"<<std::endl;
					}

		std::cout<<"\n";
	}

	if(reverseCheck){
		std::cout<<"\nReverse readings"<<std::endl;

		for (int i = NUM_MOTORS-1; i >-1; i--) {
			for(int j= 0; j<CHECK_POSITIONS; j++){
				//compare indexes 0 and 1. Are they within 11 (1 degree) of eachother? good, lets compare 2 to be sure

					tempPositions[j]= dxl_read_word(i+1, PRESENT_POSITION);
					std::cout<<tempPositions[j]<<"  ";
			}
			tempRevPositions[i] = errorCheck(tempPositions, CHECK_POSITIONS);//holds value of the error checking results

			/*check to see if motor reading falls within CCW and CW limits*/
			if(getBoundedPosition(tempRevPositions[i], i+1)<0){
				std::cout<<"Motor "<<i<<" "<<"is outside of its range of motion!"<<std::endl;
			}
			std::cout<<"\n";
			// Needed so that we do not try to resize vector to a negative value
		}

		for(int i=0; i<badMotorSize-1; i++){
			int toReplace= badMotorList[i];
			currentPose[toReplace]= tempRevPositions[toReplace]; //each value of badMotor list specifies the an index inside currentPose qhich needs to be placed
		}
	}

	/*This loop will print out the final positions at the moment in position values and degrees, and radians*/
	for (int i = 0; i < NUM_MOTORS; i++) {
		int degrees= convDegrees(currentPose[i], i);
		double radians= degrees*RADIANS_PER_DEGREE;
		if(i==0){
			std::cout<<"Motor	  Position    Degrees	 Radians"<<std::endl;
		}

		std::cout<<i+1 <<"          "<<currentPose[i]<<"          "<< degrees <<"          "<<radians<<std::endl;
		if((currentPose[i]<0) || (currentPose[i]>4095)){
			badPose= true;
		}
	}
	if(badPose){
		std::cout<<"UNSAFE MOTOR READINGS!\n Try again by releasing all (ra)\n and then enabling all (ea)\n"<<std::endl;
	}

}
/*algorithm for excluding outliers in "size" number of motor reads*/
int MotorController::errorCheck(int a[], int size){
	int prelimScan=0;
	int check1;
	int check2;
	for(int i= 0; i<size-3; i++){  // we use (i<size-3) because we are looking ahead 3 indices at most. i.e looking at i=0 to i+3.

		if((a[i]>=0) && (a[i]<=4095) && (a[i+1]>=0) && (a[i+1] <=4095) && (a[i+2]>=0) && (a[i+2] <=4095)){
			check1= abs(a[i]-a[i+1]);
			check2= abs(a[i+1]-a[i+2]);
			if((check1<=POSITION_TOLERANCE) && (check2<=POSITION_TOLERANCE)){// We will fist check to see if any xxx are the same.
				prelimScan= a[i];
				return prelimScan;
			}
		}
		else if((a[i+3]>=0) && (a[i+3] <=4095)) {
			check1= abs(a[i]-a[i+1]);
			check2= abs(a[i+1]-a[i+3]);
			if((check1<=POSITION_TOLERANCE) && (check2<=POSITION_TOLERANCE)){	//check to see if any xx_x are the same.
				prelimScan= a[i];
				return prelimScan;
			}
			check1= abs(a[i]-a[i+2]);
			check2= abs(a[i+2]-a[i+3]);
			if((check1<=POSITION_TOLERANCE) && (check2<=POSITION_TOLERANCE)){ //check to see if any x_xx are the same.
				prelimScan= a[i];
				return prelimScan;
			}
		}
	}
	std::cout<<"No combos of 3 matches"<<std::endl;

	if(prelimScan>0){
		return prelimScan;
	}

	else{		//if goodOnes array does not contain values, return error
		return -1;
	}

	return -1;
}

int MotorController::convDegrees(int pos, int index){
	int degrees;

	switch(index){
	case 0:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;

	case 1:
		degrees= -1*(pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 2:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 3:
		degrees= -1*(pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 4:
		degrees= -1*(pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 5:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 6:
		degrees= -1*(pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 7:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 8:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 9:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 10:
		degrees= (pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	case 11:
		degrees= -1*(pos-zeroPose[index])*DEGREES_PER_POSITION;
		break;
	default:
		break;
	}
	return degrees;
}


void MotorController::disableMotor(int motor) {
	dxl_write_byte(motor, TORQUE_ENABLE, 0);
}

void MotorController::enableMotor(int motor) {
	dxl_write_byte(motor, TORQUE_ENABLE, 1);
	//	std::cout << "Present Position of motor " << motor << ": " << dxl_read_word(motor, PRESENT_POSITION) << std::endl;
}

void MotorController::changePID() {
	for(int i = 0; i < NUM_MOTORS; i++) {
		dxl_write_byte(i+1, P_GAIN, P_FOR_PID);
	}
}

void MotorController::disableLeftLeg() {
	int leftLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		leftLeg[i] = 2*i + 2;
		datas[i] = 0;
	}
	sendSyncWrite(leftLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::disableRightLeg() {
	int rightLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		rightLeg[i] = 2*i + 1;
		datas[i] = 0;
	}
	sendSyncWrite(rightLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::enableRightLeg() {
	int rightLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		rightLeg[i] = 2*i + 1;
		datas[i] = 1;
	}
	sendSyncWrite(rightLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::enableLeftLeg() {
	int leftLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		leftLeg[i] = 2*i + 2;
		datas[i] = 1;
	}
	sendSyncWrite(leftLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

int MotorController::readMotorPosition(int motor) {
	return ( (int)dxl_read_word(motor, PRESENT_POSITION) );
}
void MotorController::setStatusReturnLevel(int level) {
	for (int i = 0; i < NUM_MOTORS; i++) {
		dxl_write_byte(i+1, STATUS_RETURN_LEVEL, level);
	}

}

/*this is supposed to be a high level call. It takes the new time that a step should take (user input) and passes it to editStep()*/
void MotorController::setTime(double stepTime){
	std::cout <<"Setting time for new step..." << motions[currentMotion].currentIndex<<std::endl;
	editStep(motions[currentMotion], motions[currentMotion].currentIndex, stepTime, currentPose);
}

/*edits a step inside of a motion by taking a new pose and calculating the new speeds*/
void MotorController::editStep(Motion &tempMotion, int stepIndex, double newTime, int currPos[]) {
	//[rows= steps][columns=motors]

	int finalPos;
	int initialPos;

	std::cout << "This is the step index: " << stepIndex << std::endl;

	/*Book keeping to recover the previous step. Need pos's, vel's, and time*/
	std::cout << tempMotion.time[stepIndex] << std::endl;
	if (tempMotion.time[stepIndex] != -1) {
		tempMotion.recoverTime= tempMotion.time[stepIndex];
	}
	tempMotion.time[stepIndex]= newTime;

	tempMotion.recoverPos.resize(NUM_MOTORS);//only one row becuase its a single step to recover
	tempMotion.recoverVel.resize(NUM_MOTORS);//only one row becuase its a single step to recover
	/****************/

	for(int i=0; i<NUM_MOTORS; i++){
		/* Move the current step into the recovery vectors in case the new step doesn't work out*/
		//std::cout << tempMotion.motorPositions[stepIndex][i] << std::endl;
		if (tempMotion.motorPositions[stepIndex][i] != -1) {
			tempMotion.recoverPos[i]= tempMotion.motorPositions[stepIndex][i]; //replace current step's motor positions with the input motor positions
			tempMotion.recoverVel[i]= tempMotion.motorVelocities[stepIndex][i]; //replace current step's motor positions with the input motor positions
		}

		tempMotion.motorPositions[stepIndex][i]= currPos[i]; //replace current step's motor positions with the input motor positions

		/*calculate change in position from previous step to current step*/
		if(tempMotion.currentIndex == 0){
			tempMotion.motorVelocities[stepIndex][i]= INITIAL_SPEED;
			std::cout <<"Step "<< stepIndex<< "...Motor "<< i+1<< " position saved..." <<std::endl;
		}
		else {
			initialPos= tempMotion.motorPositions[stepIndex-1][i];
			finalPos= tempMotion.motorPositions[stepIndex][i];
			//TODO Do we need these std::cout's?
			std::cout <<"Step "<< stepIndex<< "  " << initialPos << "   "<< finalPos << std::endl; //debugging
			tempMotion.motorVelocities[stepIndex][i]= SPEED_CONSTANT*abs(initialPos-finalPos)/newTime;
			std::cout <<"New Speed= "<<tempMotion.motorVelocities[stepIndex][i] <<std::endl;	//debugging
			std::cout <<"Step "<< stepIndex<< "...Motor "<< i+1<< " position saved..." <<std::endl;
		}
	}
}

void MotorController::printMotion(int printCode){

	switch(printCode){

	case 1: //print to the console
		std::cout<< motions[currentMotion].length <<std::endl;

		/*Go through all of the steps in this motion, first printing out the time*/
		for(int i= 0; i<motions[currentMotion].length; i++){
			std::cout<<"\n"<< motions[currentMotion].time[i] <<"\n"<<std::endl;

			/*then print out each motor position and speed for this step*/
			for(int j=0; j<NUM_MOTORS; j++){
				std::cout<< motions[currentMotion].motorPositions[i][j] <<"\t" <<motions[currentMotion].motorVelocities[i][j]<<std::endl;
			}
			std::cout<<"+---------+"<<std::endl;
			std::cout<<"| step "<<i<<"  |"<<std::endl;
			std::cout<<"+---------+"<<std::endl;
		}
		break;
	case 2: //print to a txt file
		std::ofstream outFile;
		outFile.open(ANIMATION_FILE);
		outFile<< motions[currentMotion].length<< "\t\n";

		//Go through all of the steps in this motion, first printing out the time
		for(int i= 0; i<motions[currentMotion].length; i++){
			outFile<<"\t\t\n"<< motions[currentMotion].time[i] <<"\t\n\t\t\n";

			//then print out each motor position and speed for this step
			for(int j=0; j<NUM_MOTORS; j++){
				outFile<< motions[currentMotion].motorPositions[i][j] <<"\t" <<motions[currentMotion].motorVelocities[i][j]<<"\n";
			}

		}
		outFile.close();
		initialize(); //reinitialize all of the motions to account for the changes made to animation file
		setMotion(ANIMATION);
		std::cout<<"Success! Edited motion saved to " <<ANIMATION_FILE<< "\n\n";
		break;
	}
}

void MotorController::setReturnDelayTime() {
	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = NEW_RETURN_DELAY_TIME;
	}
	sendSyncWrite(motors, NUM_MOTORS, RETURN_DELAY_TIME, BYTE, datas);
}

void MotorController::addMotion() {
	currentMotion = TEMP_ANIMATION;
	Motion newMotion;
	motions[currentMotion] = newMotion;
	motions[currentMotion].length = motions[currentMotion].length + 1;
	motions[currentMotion].currentIndex = 0;
	motions[currentMotion].currPos.resize(NUM_MOTORS);
	motions[currentMotion].motorPositions.resize(motions[currentMotion].length);
	motions[currentMotion].motorVelocities.resize(motions[currentMotion].length);
	motions[currentMotion].recoverPos.resize(motions[currentMotion].length);
	motions[currentMotion].recoverVel.resize(motions[currentMotion].length);
	motions[currentMotion].time.resize(motions[currentMotion].length);
	motions[currentMotion].motorPositions[motions[currentMotion].currentIndex].resize(NUM_MOTORS);
	motions[currentMotion].motorVelocities[motions[currentMotion].currentIndex].resize(NUM_MOTORS);

	motions[currentMotion].time[0] = -1;
	motions[currentMotion].motorPositions[0][0] = -1;
	motions[currentMotion].motorVelocities[0][0] = -1;
	motions[currentMotion].currPos[0] = -1;

	std::cout << motions[currentMotion].time[0] << std::endl;
}

void MotorController::addMotionStep() {

	std::cout << "Adding new step: " << motions[currentMotion].length << std::endl;
	motions[currentMotion].length = motions[currentMotion].length + 1;
	motions[currentMotion].currPos.resize(motions[currentMotion].length);
	motions[currentMotion].currentIndex++;
	motions[currentMotion].motorPositions.resize(motions[currentMotion].length);
	motions[currentMotion].motorVelocities.resize(motions[currentMotion].length);
	motions[currentMotion].recoverPos.resize(motions[currentMotion].length);
	motions[currentMotion].recoverVel.resize(motions[currentMotion].length);
	motions[currentMotion].time.resize(motions[currentMotion].length);
	motions[currentMotion].motorPositions[motions[currentMotion].currentIndex].resize(NUM_MOTORS);
	motions[currentMotion].motorVelocities[motions[currentMotion].currentIndex].resize(NUM_MOTORS);

	motions[currentMotion].time[motions[currentMotion].currentIndex] = -1;
	motions[currentMotion].motorPositions[motions[currentMotion].currentIndex][0] = -1;
	motions[currentMotion].motorVelocities[motions[currentMotion].currentIndex][0] = -1;
	motions[currentMotion].currPos[motions[currentMotion].currentIndex] = -1;
}


void MotorController::decrementIndex() {
	motions[currentMotion].currentIndex--;
}
int MotorController::getBoundedPosition(int pos, int id){

	switch(id){
	case 1:
		if(pos>=M1_CW && pos<=M1_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 2:
		if(pos>=M2_CW && pos<=M2_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 3:
		if(pos>=M3_CW && pos<=M3_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 4:
		if(pos>=M4_CW && pos<=M4_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 5:
		if(pos>=M5_CW && pos<=M5_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 6:
		if(pos>=M6_CW && pos<=M6_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 7:
		if(pos>=M7_CW && pos<=M7_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 8:
		if(pos>=M8_CW && pos<=M8_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 9:
		if(pos>=M9_CW && pos<=M9_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 10:
		if(pos>=M10_CW && pos<=M10_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 11:
		if(pos>=M11_CW && pos<=M11_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 12:
		if(pos>=M12_CW && pos<=M12_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 13:
		if(pos>=M13_CW && pos<=M13_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 14:
		if(pos>=M14_CW && pos<=M14_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 15:
		if(pos>=M15_CW && pos<=M15_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 16:
		if(pos>=M16_CW && pos<=M16_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 17:
		if(pos>=M17_CW && pos<=M17_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 18:
		if(pos>=M18_CW && pos<=M18_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 19:
		if(pos>=M19_CW && pos<=M19_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 20:
		if(pos>=M20_CW && pos<=M20_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 21:
		if(pos>=M21_CW && pos<=M21_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 22:
		if(pos>=M22_CW && pos<=M22_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 23:
		if(pos>=M23_CW && pos<=M23_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	case 24:
		if(pos>=M24_CW && pos<=M24_CCW){
			return pos;
		}
		else{
			return -1;
		}
		break;
	default:
		return -1;
		break;
	}
	return -1;
}
